import vue from '../main'
import { Message, Notification  } from 'element-ui';
import store from '../store/index'
import { ros, OBJ, viewer } from './init'
import { showNewMap } from './map'
import { showWaypoint } from './waypoint'
import { showTrajectoryAdd } from './trajectory'
import { queryBranch } from './branch'


/**
 * listenTF
 * map: 地图坐标，固定坐标系
 * odom: 机器人初始坐标，固定坐标系
 * base_link：机器人中心位置坐标
 * **/
export function listenTf() {
    return new Promise(function (resolve, reject) {
        //从ros中订阅TFS
        var tfClient = new ROSLIB.TFClient({
            ros: ros,
            fixedFrame: 'map',
            angularThres: 0.01,
            transThres: 0.01
        });
        tfClient.subscribe('odom', (tf) => {
            OBJ.tfMsg['map2odom'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        tfClient.subscribe('base_footprint', (tf) => {
            OBJ.tfMsg['map2base_footprint'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        tfClient.subscribe('base_laser', (tf) => {
            OBJ.tfMsg['map2base_laser'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        var tfClient2 = new ROSLIB.TFClient({
            ros: ros,
            fixedFrame: 'base_link',
            angularThres: 0.01,
            transThres: 0.01
        });
        tfClient2.subscribe('base_laser', (tf) => {
            OBJ.tfMsg['base_link2base_laser'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        resolve('tfClient')
    })
}

/**
 * 获取当前引导步骤
 * **/
export function guideStore(){
    return new Promise(function(resolve, reject){
        let guide = window.localStorage.getItem("guide");
        let guideName = window.localStorage.getItem("guideName");
        if(!guide){
            store.commit("guideModule", true);
            switch (guideName){
                case null:
                    showAuthor(true);
                    break;
                case 'author':
                    //判断用户是否完成引导动作
                    showNewMap(true);
                    break;
                case 'map':
                    showWaypoint(true);
                    break;
                case 'waypoint':
                    showTrajectoryAdd(true);
                    break;
                case 'trajectory':
                    showMore(true);
                    break;
                case 'more':
                    guideLast(true);
                    break;
                deflate:
                    break;
            }
        }else{
            store.commit("guideModule", false);
        }
        resolve('guide')
    })
}

/**
 * 显示授权码
 * flag  true:显示   false:隐藏
 * 如果用户初次使用，执行引导程序
 * **/
export function showAuthor(flag){
    let guide = window.localStorage.getItem('guide');
    if(flag){
        store.commit('switchAutherModule', true);
        if(!guide){
            store.commit("guideIndex", 0);
        }
    }else{
        store.commit('switchAutherModule',false);
    }
}

/**
 * 提交授权密码
 * 验证成功，授权缓存authorizationPassword
 * 通过检测缓存authorizationPassword来判断是否已授权
 * authorizationPassword:6022  developAuthorization:343
 * 引导num++
 * **/
export function submitAuthor(authorPassword) {
    if (authorPassword === store.state.authorizationPassword) {
        store.commit('switchAutherModule', false);
        store.commit('userRole', 'Administrator');
        var storage = window.localStorage;
        storage.setItem('authorizationPassword', authorPassword);//缓存密码
        storage.setItem('role', 'Administrator');//缓存身份
        storage.setItem('guideName', 'author');//缓存当前授权码步骤
        showAuthor(false);
        let guide = window.localStorage.getItem('guide');
        if(!guide){
            showNewMap(true);
        }
        return true;
    } else if(authorPassword === store.state.developAuthorization){
        store.commit('switchAutherModule', false);
        store.commit('userRole', 'developer');
        var storage = window.localStorage;
        storage.setItem('authorizationPassword', authorPassword);
        storage.setItem('role', 'developer');
        storage.setItem('guideName', 'author');//缓存当前授权码步骤
        showAuthor(false);
        let guide = window.localStorage.getItem('guide');
        if(!guide){
            showNewMap(true);
        }
        return true;
    } else {
        Notification({
            title: vue.$t("titles.error"),
            message: vue.$t("messages.pswError"),
            type: 'error',
            position: 'top-left',
            offset: 100
        });
        var storage = window.localStorage;
        storage.removeItem('authorizationPassword');
        return false;
    }
};

/**
 * 显示更多
 * flag :true显示  :false:隐藏
 * 如果用户引导未结束，显示更多
 * **/
export function showMore(flag){
    let guide = window.localStorage.getItem('guide');
    if(flag){
        store.commit("guideEndModule", true);
        if(!guide){
            store.commit("guideIndex", 4);
        }
    }else{
        store.commit("guideEndModule", false);
    }
}

/**
 * 关闭引导
 * 显示更多模态框
 * **/
export function closeGuide(){
    store.commit('switchMoreBtns', true);
    window.localStorage.setItem("guide", true);
    guideLast(false);
}

/**
 * 引导最后函数
 * 隐藏引导，记录用户已经引导完成
 * 隐藏引导模态框
 * 显示透明模态框
 * **/
export function guideLast(flag){
    if(!flag){
        showMore(false);
        store.commit("guideModule", false);
    }
}


/**
 * 全局路径
 * **/
export function globalPlan(boole = true) {
    return new Promise(function(resolve, reject){
        if (boole) {
            var path = null;
            OBJ.topic.globalPathTopic.subscribe(function (data) {
                if (path != null) {
                    viewer.scene.removeChild(path);
                }
                if (data.poses.length > 0) {
                    path = new ROS2D.PathShape({
                        path: data,
                        strokeSize: 0.17,
                        strokeColor: createjs.Graphics.getRGB(69, 182, 69, 0.9),
                    });
                    OBJ.globalPlanStage = path;
                    viewer.scene.addChild(path);
                }
            });
        } else {
            OBJ.topic.globalPathTopic.unsubscribe();
            viewer.scene.removeChild(OBJ.globalPlanStage)
        }
        resolve()
    })
}

/**
 * 局部路径
 * **/
export function tabLocalPlan(boole = true) {
    return new Promise(function(resolve, reject){
        if (boole) {
            var path = null;
            OBJ.topic.tabLocalPathTopic.subscribe(function (data) {
                if (path != null) {
                    viewer.scene.removeChild(path);
                }
                if (data.poses.length > 0) {
                    path = new ROS2D.PathShape({
                        path: data,
                        strokeSize: 0.17,
                        strokeColor: createjs.Graphics.getRGB(245, 108, 108, 0.9),
                    });
                    OBJ.tabLocalPlanStage = path;
                    viewer.scene.addChild(path);
                }
            });
        } else {
            OBJ.topic.tabLocalPathTopic.unsubscribe();
            viewer.scene.removeChild(OBJ.tabLocalPlanStage);
        }
        resolve("local plan")
    })
}

/**
 * display laserScan
 * **/
export function displayLaserScan(boole, options) {
    if (boole) {
        OBJ.topic.laserScanTopic.subscribe((message) => {
            OBJ.laserScanMsg = message;
            if (OBJ.laserScanStage) {
                viewer.scene.removeChild(OBJ.laserScanStage);
            }

            //激光坐标
            let laserScanPose = {
                x: OBJ.tfMsg.map2base_laser.transform.translation.x,
                y: -OBJ.tfMsg.map2base_laser.transform.translation.y,
                rotation: viewer.scene.rosQuaternionToGlobalTheta(OBJ.tfMsg.map2base_laser.transform.rotation)
            }
            OBJ.laserScanPose = laserScanPose;
            let skipNum = 10;
            let laserScanArr = [];
            for (var i = 0; i < message.ranges.length; i += skipNum) {
                if (message.ranges[i] === 'inf' || message.ranges[i] === 'nan'
                    || message.ranges[i] < message.range_min
                    || message.ranges[i] > message.range_max) {
                    continue;
                }
                var angle = message.angle_min + message.angle_increment * i;//激光线间隔弧度
                var rotationPI = laserScanPose.rotation / 180 * Math.PI;//激光弧度
                //激光线弧度
                let degree = -angle + rotationPI;
                //Math.cos转换为角度，根据角度计算边长
                let baseScanX = Math.cos(degree) * message.ranges[i];
                let baseScanY = Math.sin(degree) * message.ranges[i];
                //加激光坐标，得出激光点坐标
                let mapScanX = laserScanPose.x + baseScanX;
                let mapScanY = laserScanPose.y + baseScanY;
                laserScanArr.push({
                    x: mapScanX,
                    y: mapScanY
                })
            }
            OBJ.laserScanStage = laserScanShape(laserScanArr, {
                fillColor: '#ff9800'
            });
            viewer.scene.addChild(OBJ.laserScanStage);
        })
    } else {
        OBJ.topic.laserScanTopic.unsubscribe();
        viewer.scene.removeChild(OBJ.laserScanStage);
    }
}

//激光绘制
function laserScanShape(laserScanMessage, options) {
    var container = new createjs.Container();
    var size = 0.05;
    var fillColor = createjs.Graphics.getRGB(255, 153, 0, 1);
    var graphics = new createjs.Graphics();
    graphics.beginFill(fillColor);
    for (var i = 0; i < laserScanMessage.length; i++) {
        var posRos = laserScanMessage[i];
        graphics.moveTo(posRos.x, posRos.y);
        graphics.drawCircle(posRos.x, posRos.y, size);
    }
    graphics.endFill();
    var shape = new createjs.Shape(graphics);
    container.addChild(shape);
    return container;
}


/**
 * 显示/隐藏轮廓
 * **/
export function displayFootprint(boole) {
    if (boole) {
        OBJ.topic.footprintTopic.subscribe(message => {
            if (OBJ.footprintStage) {
                viewer.scene.removeChild(OBJ.footprintStage);
            }
            OBJ.footprintStage = footprintShape(message, {
                strokeColor: '#8bc34a'
            });
            viewer.scene.addChild(OBJ.footprintStage);
        })
    } else {
        OBJ.topic.footprintTopic.unsubscribe();
        viewer.scene.removeChild(OBJ.footprintStage);
    }
}

//轮廓绘制
function footprintShape(footprint, options) {
    var container = new createjs.Container();
    if (footprint.polygon.points.length < 3) {
        return container;
    }
    var strokeSize = options.strokeSize || 0.05;
    var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 255, 0, 1);
    var graphics = new createjs.Graphics();
    graphics.setStrokeStyle(strokeSize);
    graphics.beginStroke(strokeColor);
    var posRos = footprint.polygon.points[0];
    // var posPx = viewer.scene.rosQuaternionToGlobalTheta(posRos);
    var posPxOrg = posRos;
    graphics.moveTo(posRos.x, -posRos.y);
    for (var i = 1; i < footprint.polygon.points.length; i++) {
        posRos = footprint.polygon.points[i];
        graphics.lineTo(posRos.x, -posRos.y);
        graphics.moveTo(posRos.x, -posRos.y);
    }
    graphics.lineTo(posPxOrg.x, -posPxOrg.y);
    graphics.endStroke();
    var shape = new createjs.Shape(graphics);
    container.addChild(shape);
    return container;
}


/**
 * 低电量警告
 * **/
export function batteryWarning(){
    if(store.state.batteryState == PARAMS.butteryStatus.charging){
        PARAMS.butteryStatus.chargingFlag = true;
        clearInterval(PARAMS.butteryStatus.timeoutObj);
        return
    }else if(store.state.batteryState == PARAMS.butteryStatus.warning){
        if(PARAMS.butteryStatus.chargingFlag) return;
        PARAMS.butteryStatus.chargingFlag = true;
        PARAMS.butteryStatus.timeoutObj = setInterval(() =>{
            Notification({
                title: "警告",
                message: "电池电量过低，请及时充电。",
                type: "warning",
                position: 'top-left',
                offset: 100,
                duration: 0
            });
        }, PARAMS.butteryStatus.timeout)
    }
}


/**
 * 防抖
 * **/
export function debounce(fn, wait) {
    var timer = null;
    return function () {
        var context = this
        var args = arguments
        if (timer) {
            clearTimeout(timer);
            timer = null;
        }
        timer = setTimeout(function () {
            fn.apply(context, args)
        }, wait)
    }
}


/**
 * 通知
 * title
 * message
 * type
 * type=true则查询分支
 * **/
export function noticeAndMessige(param){
    if(param.type == "success"){
        queryBranch();
    }
    Notification({
        title: param.title,
        message: param.message,
        type: param.type,
        position: 'top-left',
        offset: 100
    });
    Message({
        message: param.message,
        type: param.type,
        center: true,
        duration: 0,
        showClose: false,
        customClass: "bottomTaskStatus"
    });
}

/**
 * 正则匹配特殊符号
 * */
export function checkStr(str) {
    var myReg = /[`~!@#$%^&*()\-+=<>?:"{}|,.\/;'\\[\]·~！@#￥%……&*（）——\-+={}|《》？：“”【】、；‘'， 。、]/im;
    return !myReg.test(str);
}


/**
 * 判断用户是否授权
 * **/
export function verifyAuthorization(){
    if(!window.localStorage.getItem('authorizationPassword')){
        Notification({
            title: vue.$t("titles.warning"),
            message: vue.$t("messages.notPermission"),
            type: 'warning',
            position: 'top-left',
            offset: 100
        });
        return false;
    }
    if(window.localStorage.getItem('role') === "developer"){
        store.commit("userRole", "develop");
    }
    return true
}

/**
 *判断当前设备类型
 * **/
export function _isMobile() {
    let flag = navigator.userAgent.match(/(phone|pad|pod|iPhone|iPod|ios|iPad|Android|Mobile|BlackBerry|IEMobile|MQQBrowser|JUC|Fennec|wOSBrowser|BrowserNG|WebOS|Symbian|Windows Phone)/i)
    return flag;
}

//quaternion > yaw
export function quaternionToYaw(orientation, ignoreXY) {
    var rotation = orientation;
    var numerator;
    var denominator;
    if (ignoreXY) {
        numerator = 2 * rotation.w * rotation.z;
        denominator = 1 - 2 * Math.pow(rotation.z, 2);
    } else {
        numerator = 2 * (rotation.w * rotation.z + rotation.x * rotation.y);
        denominator = 1 - 2 * (Math.pow(rotation.y, 2) + Math.pow(rotation.z, 2));
    }
    var yaw = Math.atan2(numerator, denominator);
    return yaw;
};


/**
 * 有效的四元数
* **/
export function isQuaternionValid(orientation, threshold) {
    var threshold = threshold || 0.0001;
    var sum = 0;
    for (var key in orientation) {
        if (orientation.hasOwnProperty(key)) {
            sum += orientation[key] * orientation[key];
        }
    }
    return (Math.abs(sum - 1) < threshold);
}


/**
 * 转换为四元数
 * **/
export function thetaToQuaternion(theta) {
    return {
        x: 0,
        y: 0,
        z: Math.sin(theta / 2),
        w: Math.cos(theta / 2)
    };
};

